#!/usr/bin/python
# coding=UTF-8
import cv2
import math
import numpy as np
import time
import select
import logging
import threading
import multiprocessing
from camera import *  # 摄像头对象实例化改到了main文件
from serialport import *
from lock import *  # 巡航进程停止位
logging.basicConfig(level = logging.DEBUG, format = '%(levelname)s: %(message)s')
# logging.basicConfig函数对日志的输出格式及方式做相关配置


def Find_1st_not_xx(xx, res):
    """
    在列表中找到第一个符合条件的下标
    :param xx: 要比较的数
    :param res:
    :return:
    """
    if xx == 159:
        for i in range(len(res)):
            if res[i] < 159:
                return i
    else:
        for i in range(len(res)):
            if res[i] > 0:
                return i


class DriverClass:
    right_res = []
    left_res = []
    mid_res = []
    left_loss_index, right_loss_index = [], []  # 记录边界刚好是图像边界的情况。用于判断十字
    mid_160_res = []  # 用来存储左右两边都丢线的情况
    IMAGE_X, IMAGE_Y = 160, 120
    IMAGE_X_minus_1 = IMAGE_X - 1
    IMAGE_X_half = IMAGE_X // 2
    imgMORPH = 0
    valid_lines = 50

    def __init__(self):
        # 提取赛道_YELLOW
        self.lower_yellow = np.array([22, 43, 46])  # 黄色的下阈值
        self.upper_yellow = np.array([34, 255, 255])

        STOPPED_cruise.value = False  # 线程结束标志位
        self.first = True

    def Image_Set(self):
        """
        :return: 二值化图像和轮廓的数量
        """
        imagine = front_cam_Object.read()
        imgresize = cv2.resize(imagine, (self.IMAGE_X, self.IMAGE_Y))
        imgHSV = cv2.cvtColor(imgresize, cv2.COLOR_BGR2HSV)  # HSV空间

        imgThresholded = cv2.inRange(imgHSV, self.lower_yellow, self.upper_yellow)

        # 形态学操作滤除噪点
        element = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3))
        imgdilate = cv2.dilate(imgThresholded, element)  # 膨胀
        self.imgMORPH = cv2.morphologyEx(imgdilate, cv2.MORPH_CLOSE, element)  # 先腐蚀后膨胀操作，原为腐蚀操作

        # 查找轮廓
        _, contours, hierarchy = cv2.findContours(self.imgMORPH, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
        # _, contours, hierarchy = cv2.findContours(imgMORPH, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)

        pixel_number_threshold = 75  # 阈值，根据点的数量用来筛选轮廓 #TODO
        for i in range(len(contours) - 1, 0, -1):  # 从后面开始删除就没事
            if contours[i].size < pixel_number_threshold:
                del contours[i]
        num_of_contours = len(contours)  # 得到轮廓的数量
        # logging.debug('count of contours is:{}'.format(num_of_contours))
        # print('count of contours is :', num_of_contours)
        return self.imgMORPH, num_of_contours

    def Find_Change(self, imgMORPH, tmp_y, start_x=80, direction=1, x_max=160):  # direction为1，则是向右 ，处理每一行
        """
        :param imgMORPH: 二值化图像
        :param tmp_y: 指定在哪一行执行
        :param start_x: 指定开始查找的纵坐标
        :param direction: 指定方向
        :param x_max:
        :return: 图像的最大长度
        """
        right_have_found, left_have_found = 0, 0  # 用于判断是否找到黑白边界
        if direction == 1:  # 向右查找
            try:  # 在左转弯的时候容易数组下标溢出
                for x in range(start_x, x_max, 3):  # 3个点跳着找，速度快
                    if imgMORPH[tmp_y][x] == 255:
                        if imgMORPH[tmp_y][x - 1] == 0:
                            self.right_res.append(x)
                            right_have_found = 1
                            break
                        elif imgMORPH[tmp_y][x - 2] == 0:
                            self.right_res.append(x - 1)
                            right_have_found = 1
                            break
                        elif imgMORPH[tmp_y][x - 3] == 0:
                            self.right_res.append(x - 2)
                            right_have_found = 1
                            break
                if right_have_found == 0:
                    self.right_res.append(x_max - 1)  # 找不到跳变点时赋予图像的最大长度-1
                    self.right_loss_index.append(tmp_y)
                return self.right_res[-1]
            except Exception as e:
                self.right_res.append(0)
                return self.right_res[-1]
        else:  # 向左查找
            try:  # 在右弯道的时候有
                for x in range(start_x, 0, -3):
                    if imgMORPH[tmp_y][x] == 255:
                        if imgMORPH[tmp_y][x + 1] == 0:
                            self.left_res.append(x)
                            left_have_found = 1
                            break
                        elif imgMORPH[tmp_y][x + 2] == 0:
                            self.left_res.append(x + 1)
                            left_have_found = 1
                            break
                        elif imgMORPH[tmp_y][x + 3] == 0:
                            self.left_res.append(x + 2)
                            left_have_found = 1
                            break
                if left_have_found == 0:
                    self.left_res.append(0)  # 找不到跳变点时赋予0
                    self.left_loss_index.append(tmp_y)
                return self.left_res[-1]  # 返回最后加入的那个
            except Exception as e:
                self.left_res.append(self.IMAGE_X_minus_1)
                return self.left_res[-1]

    def new_findChange(self, imgMORTH, tmp_y, last_x1=0, last_x2=0, direction=1, x_max=160):
        right_have_found, left_have_found = 0, 0  # 用于判断是否找到黑白边界,黑色是0，白色是255
        if direction:  # 在右边查找
            try:
                if 5 <= last_x2 <= 154:
                    if imgMORTH[tmp_y][last_x2-5] == 255:
                        for x in range(last_x2-5, 0, -1):
                            if imgMORTH[tmp_y][x] == 0:
                                self.right_res.append(x)
                                right_have_found = 1
                                break
                    else:
                        for x in range(last_x2-5, last_x2+5, 1):
                            if imgMORTH[tmp_y][x] == 255:
                                self.right_res.append(x)
                                right_have_found = 1
                                break
                elif 0 <= last_x2 < 5:
                    for x in range(0, 5, 1):
                        if imgMORTH[tmp_y][x] == 255:
                            self.right_res.append(x)
                            right_have_found = 1
                            break
                    if not right_have_found:
                        self.right_res.append(0)
                        right_have_found = 1
                elif 154 < last_x2 <= self.IMAGE_X_minus_1:
                    for x in range(159, 154, -1):
                        if imgMORTH[tmp_y][x] == 0:
                            self.right_res.append(x)
                            right_have_found = 1
                            break
                if not right_have_found:
                    self.right_loss_index.append(tmp_y)
                    self.right_res.append(self.IMAGE_X_minus_1)
                return self.right_res[-1]
            except Exception as e:
                logging.error("raise an error in cruise at find right turn point")
                logging.error(e)
                self.right_res.append(self.IMAGE_X_minus_1)
                return self.right_res[-1]

        else:  # 向左边寻找跳变点
            try:
                if 5 <= last_x1 <= 154:
                    if imgMORTH[tmp_y][last_x1+5] == 255:
                        for x in range(last_x1+5, self.IMAGE_X_minus_1, 1):
                            if imgMORTH[tmp_y][x] == 0:
                                self.left_res.append(x)
                                left_have_found = 1
                                break
                    else:
                        for x in range(last_x1+5, last_x1-5, 1):
                            if imgMORTH[tmp_y][x] == 255:
                                self.left_res.append(x)
                                left_have_found = 1
                                break
                elif 0 <= last_x1 < 5:
                    for x in range(0, 5, 1):
                        if imgMORTH[tmp_y][x] == 255 and imgMORTH[tmp_y][x+1] == 0:
                            self.left_res.append(x)
                            left_have_found = 1
                            break
                elif 154 < last_x1 <= self.IMAGE_X_minus_1:
                    for x in range(self.IMAGE_X_minus_1, 154, -1):
                        if imgMORTH[tmp_y][x] == 255:
                            self.left_res.append(x)
                            left_have_found = 1
                            break
                    if not left_have_found:
                        self.left_res.append(self.IMAGE_X_minus_1)
                        left_have_found = 1

                if not left_have_found:
                    self.left_loss_index.append(tmp_y)
                    self.left_res.append(0)
                return self.left_res[-1]

            except Exception as e:
                logging.error("raise an error in cruise at find right turn point")
                logging.error(e)
                self.left_res.append(0)
                return self.left_res[-1]


    def Get_LR_Change_Points(self, imgMORTH, y_min=40, y_max=120):
        """

        :param imgMORTH:二值化图像
        :param y_min: 设置有效行的终点
        :param y_max: 设置有效行的起点
        :return: 得到左右跳变点的列表
        """
        start_x = self.IMAGE_X // 2  # 给定一个开始的查找起点
        right = left = 0
        right = self.Find_Change(imgMORTH, tmp_y=y_max-1, start_x=start_x, direction=1)
        left = self.Find_Change(imgMORTH, tmp_y=y_max-1, start_x=start_x, direction=0)
        for y in range(y_max - 2, y_min - 1, -1):
            right = self.new_findChange(imgMORTH, tmp_y=y, last_x1=0, last_x2=right, direction=1)
            left = self.new_findChange(imgMORTH, tmp_y=y, last_x1=left, last_x2=0, direction=0)

    def Fix_Lines(self, point_down, point_up, dir1):
        """
        用于补两个点之间的线
        :param point_down: 下面的点
        :param point_up: 上面的点
        :param dir1: 方向，有两边
        :return: 返回修正的列表
        """
        if dir1 == 0:
            k = float(self.left_res[point_up] - self.left_res[point_down]) / (point_up - point_down)  # 得到x=ky+b
            for i in range(1, point_up - point_down):
                self.left_res[point_down + i] = int(self.left_res[point_down] + k * i)  # y间隔是1时，x的间隔刚好是k
        else:
            k = float(self.right_res[point_up] - self.right_res[point_down]) / (point_up - point_down)  # 得到x=ky+b
            for i in range(1, point_up - point_down):
                self.right_res[point_down + i] = int(self.right_res[point_down] + k * i)

    def Find_Turn_Point(self, imgMORTH):
        """
        找拐点，并且完成补线操作
        :return:
        """
        LB_turn_point = 0
        LF_turn_point = 0
        RB_turn_point = 0
        RF_turn_point = 0
        len_left_res = len(self.left_res)
        len_right_res = len(self.right_res)

        for i in range(1, len_left_res - 1):  # 遍历左边的跳变点列表
            if self.left_res[i] > self.left_res[i + 1] and self.left_res[i] >= self.left_res[i - 1]:  # 符合左边边界的特点
                LB_turn_point = i
                break
        if LB_turn_point != 0:  # 找到左下拐点再找左上拐点
            for i in range(LB_turn_point + 1, len_left_res - 1):
                if (self.left_res[i] - self.left_res[i - 1]) > 5 and (self.left_res[i + 1] - self.left_res[i]) < 3:
                    LF_turn_point = i
                    break

        for i in range(1, len_right_res - 1):
            if self.right_res[i] < self.right_res[i + 1] and self.right_res[i] <= self.right_res[i - 1]:
                RB_turn_point = i
                break
        if RB_turn_point != 0:
            for i in range(RB_turn_point + 1, len_right_res - 1):
                if (self.right_res[i - 1] - self.right_res[i] > 5) and (self.right_res[i] - self.right_res[i + 1] < 3):
                    RF_turn_point = i
                    break

        if (0 < LB_turn_point < LF_turn_point) and (0 < RB_turn_point < RF_turn_point):  # 十字
            logging.debug('doing 1 in cruise')
            self.Fix_Lines(LB_turn_point, LF_turn_point, dir1=0)
            self.Fix_Lines(RB_turn_point, RF_turn_point, dir1=1)
            self.Sample_Get_Midres(len_left_res, len_right_res)

        elif (LB_turn_point + RB_turn_point) == 0:  # 上半十字
            logging.debug('doing 2 in cruise')
            for i in range(1, len_left_res - 1):
                if (self.left_res[i] - self.left_res[i - 1]) > 5 and (
                        self.left_res[i + 1] - self.left_res[i]) < 3:  # 找左上拐点  # TODO
                    LF_turn_point = i
                    break
            for i in range(1, len_right_res - 1):
                if (self.right_res[i - 1] - self.right_res[i] > 5) and (
                        self.right_res[i] - self.right_res[i + 1] < 3):  # 找右上拐点
                    RF_turn_point = i
                    break
            if (LF_turn_point + RF_turn_point) == 0:
                self.Get_LR_Change_Points(imgMORTH, y_min=90, y_max=100)  # 再找50行 TODO：参数
                for i in range(100, len(self.left_res) - 1):
                    if (self.left_res[i] - self.left_res[i - 1]) > 5 and (
                            self.left_res[i + 1] - self.left_res[i]) < 3:  # 找左上拐点
                        LF_turn_point = i
                        break
                for i in range(100, len(self.right_res) - 1):
                    if (self.right_res[i - 1] - self.right_res[i] > 5) and (
                            self.right_res[i] - self.right_res[i + 1] < 3):  # 找右上拐点
                        RF_turn_point = i
                        break
            if LF_turn_point != 0 and RF_turn_point != 0:  # 上半十字
                for i in range(0, LF_turn_point):
                    self.left_res[i] = self.left_res[LF_turn_point]
                for i in range(0, RF_turn_point):
                    self.right_res[i] = self.right_res[RF_turn_point]
                self.Sample_Get_Midres(len_left_res, len_right_res)
            else:
                if LF_turn_point != 0:  # 上半十字识别到0/1个拐点
                    for i in range(0, LF_turn_point):
                        self.left_res[i] = self.left_res[LF_turn_point]
                if RF_turn_point != 0:
                    for i in range(0, RF_turn_point):
                        self.right_res[i] = self.right_res[RF_turn_point]
                self.Sample_Get_Midres(len(self.left_res), len(self.right_res))

        elif LB_turn_point != 0 and RB_turn_point != 0:  # 下十字有，上十字不一定有
            logging.debug('doing 3 in cruise')
            if LF_turn_point == 0:
                for i in range(LB_turn_point, len_left_res):
                    self.left_res[i] = self.left_res[LB_turn_point]
            if RF_turn_point == 0:
                for i in range(RB_turn_point, len_right_res):
                    self.right_res[i] = self.right_res[RB_turn_point]
            if LF_turn_point > 0:
                self.Fix_Lines(LB_turn_point, LF_turn_point, 0)
            if RF_turn_point > 0:
                self.Fix_Lines(RB_turn_point, RF_turn_point, 1)
            self.Sample_Get_Midres(len_left_res, len_right_res)
        else:
            logging.debug('doing 4 in cruise')
            self.Sample_Get_Midres(len_left_res, len_right_res)

    def Sample_Get_Midres(self, len_left_res, len_right_res):
        """
         获取中线坐标
        :param len_left_res:
        :param len_right_res:
        :return:
        """
        tmp_len = min(len_left_res, len_right_res)
        if tmp_len == 0:
            self.mid_res.append(self.IMAGE_X_half)  # 只有一个
        else:
            for i in range(tmp_len):
                self.mid_res.append((self.left_res[i] + self.right_res[i]) // 2)  # 加入左右边界的中点

    def Line1_Get_Mid(self, index1, left_res, right_res, dir=1):
        """
        一边赛道丢线的处理
        :param index1: 第一个不为边界0/319的点
        :param left_res:
        :param right_res:
        :param dir: 方向
        :return:
        """
        if dir == 1:  # 左边丢线
            for i in range(index1 + 1):  # 0 到 index1
                self.mid_res.append((left_res[i] + right_res[i]) // 2)
            error = (right_res[index1] - left_res[index1]) // 2
            for i in range(index1 + 1, self.valid_lines):  # 取到50个
                tmp_mid = right_res[i] - error
                if tmp_mid < 0:
                    return i  # 返回等于0的坐标
                else:
                    self.mid_res.append(tmp_mid)
        else:  # 右边丢线
            for i in range(index1 + 1):  # 0 到 index1
                self.mid_res.append((left_res[i] + right_res[i]) // 2)
            error = (right_res[index1] - left_res[index1]) // 2
            for i in range(index1 + 1, self.valid_lines):  # 取到100个
                tmp_mid = left_res[i] + error
                if tmp_mid > self.IMAGE_X_minus_1:
                    return i  # 返回等于0的坐标
                else:
                    self.mid_res.append(tmp_mid)

    def Analysis_LR_Arr(self, imgMORTH):
        """
        根据丢线的情况判别
        :return:
        """
        len_left_loss = len(self.left_loss_index)
        len_right_loss = len(self.right_loss_index)
        # logging.debug('len_left_loss:{}'.format(len_left_loss))
        # logging.debug('len_right_loss:{}'.format(len_right_loss))
        if len_left_loss > 20 > len_right_loss:  # 左边丢线右边不丢线 #TODO
            index = Find_1st_not_xx(self.IMAGE_X_minus_1, self.right_res)  # 找到右边界第一个不为319的点
            logging.debug('doing left loss in cruise')
            self.Line1_Get_Mid(index1=index, left_res=self.left_res, right_res=self.right_res, dir=1)  # 弯道处找中线的函数

        elif len_right_loss > 30 > len_left_loss:  # 右边丢线左边不丢线 #TODO
            index = Find_1st_not_xx(0, self.left_res)
            logging.debug('doing right loss in cruise')
            self.Line1_Get_Mid(index1=index, left_res=self.left_res, right_res=self.right_res, dir=0)
        elif len_right_loss > 15 and len_left_loss > 15:  # TODO

            self.Find_Turn_Point(imgMORTH)
        else:
            self.Sample_Get_Midres(len(self.left_res), len(self.right_res))

    def Draw_Dst(self, left_res, right_res, mid_res, imgMORPH):
        """
        画线函数
        :param left_res:
        :param right_res:
        :param mid_res:
        :param imgMORPH:
        :return:
        """
        try:
            for i, left in enumerate(self.left_res):
                imgMORPH[119 - i][left + 3] = 255
            for i, right in enumerate(self.right_res):
                imgMORPH[119 - i][right - 3] = 255
            for i, mid in enumerate(self.mid_res):
                imgMORPH[self.IMAGE_Y - 1 - i][mid] = 255
        except Exception as e:
            logging.error('draw have a error in cruise')
            logging.error(e)

    def Mid_Error_Cal(self):
        """
        得到中线的偏差
        :return:
        """
        len0 = len(self.mid_res)
        if len0 == 0:
            mid_error = 0
            # print('cal error,len of mid_res is 0')
        else:
            mid_error = (sum(
                self.mid_res[:len0 + 1]) - self.IMAGE_X_half * len0) // len0  # 不足50个时，mid_error小于0时，车子是左偏的，要向右矫正
        # 偏差限幅
        if mid_error > 100:
            mid_error = 100
        elif mid_error < -100:
            mid_error = -100
        return mid_error

    def drive_auto(self):

        # while not STOPPED_cruise.value:
        while True:
            if STOPPED_cruise.value:
                pass
            else:
                try:  # 防止读到空图报错
                    print(' ')
                    start = time.time()
                    imgMORTH, g_num_of_contours = self.Image_Set()
                    self.Get_LR_Change_Points(imgMORTH, y_min=70, y_max=self.IMAGE_Y)  # TODO
                    if g_num_of_contours == 0:
                        logging.debug('0 contours? in cruise')
                        continue
                    else:
                        self.Analysis_LR_Arr(imgMORTH)
                        self.Draw_Dst(self.left_res, self.right_res, self.mid_res, imgMORTH)
                        g_mid_error = self.Mid_Error_Cal()
                        if STOPPED_cruise.value == False:   #stop后不要再发数据
                            eb_serial_Object.write_data([0xAA, 0xC1, 0, 0, g_mid_error + 100, 0xFF])  # 加上100，变成正数，发送偏差数据！！
                        logging.debug("cruise->mid_error:{}".format(g_mid_error))
                        self.right_res.clear()
                        self.left_res.clear()
                        self.mid_res.clear()
                        self.left_loss_index.clear()
                        self.right_loss_index.clear()  # 记录边界刚好是图像边界的情况。用于判断十字
                        self.mid_160_res.clear()  # 用来存储左右两边都丢线的情况
                        # cv2.imshow('dst', imgMORTH)
                        # cv2.waitKey(1)
                        logging.debug("cruise->spend_time:{}".format(time.time() - start))
                    
                except Exception as e:
                    logging.error("raise an error at drive_auto in cruise")
                    logging.error(e)
                    pass

    """
    functions related to multiProcess
    """

    def start(self):
        if self.first:
            STOPPED_cruise.value = False
            multiprocessing.Process(target=self.drive_auto).start()  # drive_auto
            self.first = False
        else:
            STOPPED_cruise.value = False

    def stop(self):
        STOPPED_cruise.value = True


cruise_Object = DriverClass()


